package cn.swing.main.srv.cv.utils;

import cn.swing.main.srv.cv.model.TennisPoint;
import lombok.Getter;
import org.apache.commons.math3.filter.DefaultMeasurementModel;
import org.apache.commons.math3.filter.DefaultProcessModel;
import org.apache.commons.math3.filter.KalmanFilter;
import org.apache.commons.math3.filter.MeasurementModel;
import org.apache.commons.math3.filter.ProcessModel;
import org.apache.commons.math3.linear.ArrayRealVector;
import org.apache.commons.math3.linear.MatrixUtils;
import org.apache.commons.math3.linear.RealMatrix;
import org.apache.commons.math3.linear.RealVector;

public class TennisKalmanFilter {

    @Getter
    KalmanFilter filter;

    public TennisKalmanFilter(TennisPoint p) {
        initFilter(p);
    }

    /**
     * 初始化卡尔曼滤波器
     *
     * 参数调优建议表：
     * 参数	建议值	说明
     * 过程噪声 (Q)	diag(0.1,0.1,1,1)	X/Y位置噪声小（运动连续），速度噪声稍大（允许变速）
     * 观测噪声 (R)	diag(5,5)	假设坐标检测误差约±5像素
     * 最大匹配距离	50像素	允许每帧最大移动50像素（应对跳跃情况）
     * 最大漏帧数	5帧	允许最多连续5帧丢失后仍保持轨迹
     * 速度变化阈值	30像素/帧²	击事件检测的加速度阈值
     * 尺寸变化阈值	2像素	落地事件检测的宽高变化阈值
     *
     * @param p
     */
    private void initFilter(TennisPoint p) {
        // 状态向量：x, y, w, h, fno
        RealVector initialState = new ArrayRealVector(new double[]
                {p.getX(), p.getY(), p.getWidth(), p.getHeight(), p.getFrameNumber()});

        // 状态协方差矩阵
        RealMatrix initialError = MatrixUtils.createRealIdentityMatrix(5).scalarMultiply(0.1);

        // 过程模型
        ProcessModel processModel = new DefaultProcessModel(
                getStateTransitionMatrix(), // F 状态转移矩阵 描述了系统的动态模型，它定义了系统在一个时刻如何从一个状态转移到下一个状态。在网球运动轨迹的示例中，A
                // 矩阵包含了位置、速度和网球的宽高信息，通过A矩阵可以描述网球在每个时刻的运动规律。
                getControlMatrix(),         // B 控制矩阵
                getProcessNoise(),          // Q 过程噪声协方差矩阵 表示了系统模型中的过程噪声，即系统模型无法完全描述的不确定性。Q
                // 矩阵的值越大，表示系统中的不确定性越高。在网球运动轨迹的示例中，Q矩阵描述了位置、速度和宽高信息的变化过程中的噪声。
                initialState,               // initial state 初始状态向量x 表示在开始时系统的状态，即网球的初始位置、速度和宽高信息。
                initialError                // initial error 状态协方差矩阵 表示初始状态估计的不确定性。P矩阵的对角线元素越大，表示对应状态变量的估计不确定性越高。
        );

        // 测量模型
        MeasurementModel measurementModel = new DefaultMeasurementModel(
                getMeasurementMatrix(),     // H 测量矩阵 将状态向量映射到测量空间，它定义了如何将状态转换为实际的测量值。在网球运动轨迹的情况下，H
                // 矩阵将状态向量映射到实际的位置和网球的宽高信息。
                getMeasurementNoise()       // R 测量噪声矩阵 表示了测量中的噪声，即传感器测量值与真实值之间的差异。R
                // 矩阵的值越大，表示传感器测量的不确定性越高。在网球运动轨迹的情况下，R矩阵描述了测量位置和宽高信息的噪声。
        );

        this.filter = new KalmanFilter(processModel, measurementModel);
    }

    public void update(TennisPoint curr) {
        // 更新卡尔曼滤波
        RealVector z = MatrixUtils.createRealVector(new double[] {curr.getX(), curr.getY(),
                curr.getWidth(), curr.getHeight(), curr.getFrameNumber()});
        filter.correct(z);
    }

    /**
     * 过程噪声矩阵 Q
     *
     * @return
     */
    private RealMatrix getProcessNoise() {
        return MatrixUtils.createRealMatrix(new double[][] {
                {0.03, 0, 0, 0, 0},
                {0, 0.03, 0, 0, 0},
                {0, 0, 0.03, 0, 0},
                {0, 0, 0, 0.03, 0},
                {0, 0, 0, 0, 0.03}
        });
    }

    /**
     * 测量噪声矩阵 R
     *
     * @return
     */
    private RealMatrix getMeasurementNoise() {
        return MatrixUtils.createRealMatrix(new double[][] {
                {0.1, 0, 0, 0, 0},
                {0, 0.1, 0, 0, 0},
                {0, 0, 0.1, 0, 0},
                {0, 0, 0, 0.1, 0},
                {0, 0, 0, 0, 0.1}
        });
    }

    /**
     * 状态转移矩阵 F
     *
     * @return
     */
    private RealMatrix getStateTransitionMatrix() {
        return MatrixUtils.createRealMatrix(new double[][] {
                {1, 0, 0, 1, 0},
                {0, 1, 0, 0, 1},
                {0, 0, 1, 0, 0},
                {0, 0, 0, 1, 0},
                {0, 0, 0, 0, 1}
        });
    }

    /**
     * 测量矩阵 H
     *
     * @return
     */
    private RealMatrix getMeasurementMatrix() {
        return MatrixUtils.createRealMatrix(new double[][] {
                {1, 0, 0, 0, 0},
                {0, 1, 0, 0, 0},
                {0, 0, 1, 0, 0},
                {0, 0, 0, 1, 0},
                {0, 0, 0, 0, 1}
        });
    }

    /**
     * 控制矩阵 B
     *
     * @return
     */
    private RealMatrix getControlMatrix() {
        return MatrixUtils.createRealIdentityMatrix(5);
    }

    public double[] getStateEstimation() {
        return filter.getStateEstimation();
    }

}
